#ifndef __PLANNER_WORK_GROUP_HPP__
#define __PLANNER_WORK_GROUP_HPP__

#include "Planner.hpp"
#include <algorithm>
#include <cmath>
#include <queue>
#include <vector>
#include <limits>

/**
 * @brief 规划器-工作组策略
 * @note 数个机器人与数个泊位分成工作组，共同产出方案；船在泊位间轮询，在事先确定的路线中选择价值最大的执行
 */
class PlannerWorkGroup : public Planner
{
private:
    int frameID = -1; // 当前帧
    const Map *map = nullptr; // 地图
    std::vector<Berth> *berths = nullptr; // 泊位
    const std::vector<Goods *> *goods_list = nullptr; // 货物
    const std::vector<BoatDispatcher> *boatDispatchers = nullptr; // 机器人调度器
    const std::vector<RobotDispatcher> *robotDispatchers = nullptr; // 船调度器
    std::vector<std::deque<int>> commingBoats = std::vector<std::deque<int>>(BERTH_NUM); // 来船时刻表（记录的是抵达时间）
    std::priority_queue<int, std::vector<int>, std::greater<int>> sellingBoats; // 前往虚拟点出售的船时刻表（记录的是抵达时间）
    std::priority_queue<std::pair<int, int>, std::vector<std::pair<int, int>>, std::greater<std::pair<int, int>>> loadingBoats; // 在泊位装货的船时刻表（记录的是离开时间和泊位距离）
    std::vector<std::list<Goods *>> berthGoods = std::vector<std::list<Goods *>>(BERTH_NUM); // 泊位可用货物表
    int minPlanTime = TIME_FRAME_MAX; // 最短方案时间（船轮港后装货、返航卖货的最短时间）
    std::vector<int> robotGroup = std::vector<int>(ROBOT_NUM, 0); // 机器人所属工作组（0为自由工作）
    std::vector<int> berthGroup = std::vector<int>(BERTH_NUM, 0); // 泊位所属工作组（0为禁用泊位）
    int workGroupNum = 0; // 导入的工作组数量
    int freeBoatsNum = 0; // 空闲船只数量

private:
    /**
     * @brief 设置工作组ID
     * @param workGroup 
     * @param id 
     */
    void setWorkGroupID(const std::pair<std::vector<int>, std::vector<int>> &workGroup, int id) {
        for (auto const &berth : workGroup.first) {
            berthGroup[berth] = id;
        }
        for (auto const &robot : workGroup.second) {
            robotGroup[robot] = id;
        }
    }
    /**
     * @brief 导入工作组
     * @param workGroup 
     */
    void loadWorkGroup(const std::pair<std::vector<int>, std::vector<int>> &workGroup) {
        setWorkGroupID(workGroup, ++workGroupNum);
    }
    /**
     * @brief 是否在同一个工作组中
     * @param robot 
     * @param berth 
     * @return true 
     * @return false 
     */
    bool robotAndBerthInSameWorkGroup(int robotID, int berthID) {
        return robotGroup[robotID] == 0 || (berthGroup[berthID] != 0 && robotGroup[robotID] == berthGroup[berthID]);
    }
    /**
     * @brief 是否在同一个工作组中
     * @param berthID1 
     * @param berthID2 
     * @return true 
     * @return false 
     */
    bool berthAndBerthInSameWorkGroup(int berthID1, int berthID2) {
        return berthGroup[berthID1] != 0 && berthGroup[berthID2] != 0 && berthGroup[berthID1] == berthGroup[berthID2];
    }
    /**
     * @brief 前往该泊位的船现在已经有多少
     * @param berthID 
     * @return int 
     */
    int getCommingBoatNum(int berthID) const {
        return commingBoats[berthID].size();
    }
    /**
     * @brief 前往该泊位的船还有多久抵达
     * @param berthID 
     * @param index 
     * @return int 
     */
    int getCommingBoatTime(int berthID, int index = 0) const {
        int commingBoatNum = getCommingBoatNum(berthID);
        if (commingBoatNum > index) {
            return commingBoats[berthID][index] - frameID;
        }
        int ret = (*berths)[berthID].distance;
        if (freeBoatsNum >= index + 1 - commingBoatNum) { // 有足够多空闲的船可以来到该泊位
            return ret; // 直接返回距离
        } else if (!sellingBoats.empty()) { // 所有船都在忙碌，但有船在送货的路上
            return ret + (frameID - sellingBoats.top()); // 假设最快到达虚拟点的船会来取货
        } else { // 所有船都在装货
            return ret + loadingBoats.top().first + loadingBoats.top().second; // 假设最早返回虚拟点的船会来取货
        }
    }
    /**
     * @brief 添加前往泊位的船
     * @param berthID 
     * @param arriveTime 
     */
    void addCommingBoat(int berthID, int arriveTime) {
        if (berthID == VIRTUAL_BERTH_ID) {
            sellingBoats.push(arriveTime);
            return;
        }
        if (berthID < 0 || berthID > BERTH_NUM) {
            return;
        }
        loadingBoats.push(std::make_pair(arriveTime + (*berths)[berthID].loadTime, (*berths)[berthID].distance));
        commingBoats[berthID].push_back(arriveTime);
    }
    /**
     * @brief 获取货物价值（用于机器人规划）
     * @param frameID 
     * @param robot
     * @param goods 
     * @param endBerth 
     * @return float 
     */
    float getGoodsValue(int frameID, const Robot *robot, const Goods &goods, const Berth &endBerth) const {
        int carryTime = PARAMS.ROBOT_MOVE_MARGIN_TIME + (*goods.distance)[robot->x][robot->y]; // 机器人取货时间
        if (!goods.isAvailable(frameID + carryTime)) { // 来不及取货，价值为0
            return 0;
        }
        int fullBoatNum = endBerth.getFullBoatNum(); // 泊位货物足够装满几艘船
        int boatTime = getCommingBoatTime(endBerth.id, fullBoatNum) + endBerth.loadTime; // 最近的前往该泊位的船抵达并完成装载的时间
        int robotTime = carryTime + map->getBerthDistance(endBerth.id, goods)+ PARAMS.ROBOT_MOVE_MARGIN_TIME; // 机器人取货并送货时间
        if (robotTime > boatTime) { // 本次运货来不及装货，需要延迟至下次装货
            robotTime = boatTime + static_cast<int>(endBerth.distance * PARAMS.ROUND_APPEND_FACTOR) + endBerth.loadTime; // 下一次取货的时间（不使用getCommingBoatTime计算，因为可能发生轮港）
            boatTime = getCommingBoatTime(endBerth.id, fullBoatNum + 1) + endBerth.loadTime; // 船时长使用getCommingBoatTime估计，不考虑轮港，因为需要计算一个baseline
        }
        if (frameID + boatTime + endBerth.distance > TIME_FRAME_MAX - 1) { // 船取完后来不及返航，不再送货
            return 0;
        }
        float value = goods.value
                    + (static_cast<float>(GOODS_LIFE_FRAMES - goods.remainLifeTime(frameID)) / GOODS_LIFE_FRAMES) * PARAMS.GOODS_LIFE_FACTOR; // 货物快消失时，给予额外价值补偿
        return value / robotTime; // 按机器人送货时间计算价值率
    }
    /**
     * @brief 获取两个泊位间的距离
     * @param berthID1 
     * @param berthID2 
     * @return int 
     */
    int getBerthDistance(int berthID1, int berthID2 = VIRTUAL_BERTH_ID) const {
        if (berthID1 == berthID2) {
            return 1; // 空等一帧
        }
        if (berthID2 == VIRTUAL_BERTH_ID) {
            return (*berths)[berthID1].distance;
        }
        if (berthID1 == VIRTUAL_BERTH_ID) {
            return (*berths)[berthID2].distance;
        }
        return BERTH_INTER_DIS;
    }
    /**
     * @brief 获取港口价值（用于船规划）
     * @param frameID 
     * @param berthID 
     * @param boat 
     * @return float 
     */
    float getBerthValue(int frameID, int berthID, const Boat *boat) const {
        int time;
        // 航程剩余时间计算
        if (boat->berthID == VIRTUAL_BERTH_ID) { // 虚拟点出发，考虑来回总时间
            time = 2 * getBerthDistance(boat->berthID, berthID) + (*berths)[berthID].loadTime; // 路程总时间
        } else { // 非虚拟点出发
            if (berthID == VIRTUAL_BERTH_ID) {// 前往虚拟点卖货，直接计算时间
                time = getBerthDistance(boat->berthID);
            } else { // 如果不是去虚拟点卖货，还要加上在另一个泊位装货、卖货的时间
                time = getBerthDistance(boat->berthID, berthID) + (*berths)[berthID].loadTime + getBerthDistance(berthID);
            }
        }
        // 判断航程是否还能执行完
        if (frameID + time > TIME_FRAME_MAX - 1) { // 任务总时长超过总帧数，来不及了
            return 0.0;
        }
        // 还原航程总时长
        if (boat->berthID != VIRTUAL_BERTH_ID) { // 在判断完余下航程时间后，计算价值使用本次航程总时长
            time += frameID - boat->leaveVirtualPointTime; // 加上本次航程前半程的时长
        }
        if (berthID == VIRTUAL_BERTH_ID) { // 去虚拟点卖货
            return static_cast<float>(boat->getValue()) / time;
        }
        int allocatedGoods = getCommingBoatNum(berthID) * boat->volume; // 泊位处已经分配给别的船的货物（后面优化时需要动态更新）
        float berthValue = (*berths)[berthID].getValue(allocatedGoods, allocatedGoods + boat->getRemainVolume());
        if (std::abs(berthValue) < std::numeric_limits<float>::epsilon()) { // 泊位的货物已经全部分给了别的船，不再前往该泊位
            return 0.0;
        }
        return (boat->getValue() + berthValue) / time;
    }

public:
    /**
     * @brief 初始化
     * @param map 地图
     * @param berths 泊位列表
     * @param goods_list 货物列表
     * @param boatDispatchers 船调度器列表
     * @param robotDispatchers 机器人调度器列表
     * @note 初始化阶段调用一次，连接到可用的地图和泊位、货物列表等，由外部保证这些数据实时刷新
     */
    void init(const Map *map, std::vector<Berth> *berths, const std::vector<Goods *> *goods_list,
        const std::vector<BoatDispatcher> *boatDispatchers,
        const std::vector<RobotDispatcher> *robotDispatchers) override {
        this->map = map;
        this->berths = berths;
        this->goods_list = goods_list;
        this->boatDispatchers = boatDispatchers;
        this->robotDispatchers = robotDispatchers;
        for (auto const &berth : *berths) {
            int planTime = berth.distance + (*boatDispatchers)[0].boat->volume / berth.velocity + 1;
            if (planTime < minPlanTime) {
                minPlanTime = planTime;
            }
        }
        minPlanTime += BERTH_INTER_DIS;
        for (auto const &group : PARAMS.WORK_GROUPS) {
            loadWorkGroup(group);
        }
    }
    /**
     * @brief 刷新一帧
     * @param frameID 当前帧
     * @param newGoodsNum 新增货物数
     * @note  每帧运行一次（可能存在跳帧），新增货物数用于倒序遍历goods_list
     */
    void refresh(int frameID, int newGoodsNum) override {
        this->frameID = frameID;
        for (auto &boat : commingBoats) { // 更新来船时刻表
            while (!boat.empty() && frameID > boat.front()) {
                boat.pop_front();
            }
        }
        while (!sellingBoats.empty() && frameID >= sellingBoats.top()) { // 更新出售时刻表
            sellingBoats.pop();
        }
        while (!loadingBoats.empty() && frameID >= loadingBoats.top().first) { // 更新装货时刻表
            loadingBoats.pop();
        }
        int cnt = 0;
        for (auto it = goods_list->rbegin(); it != goods_list->rend(); ++it) { // 将能到达的货物推入泊位货物表
            if (cnt++ >= newGoodsNum) {
                break;
            }
            int area = map->getNearestBerth(**it);
            for (int berthID = 0; berthID < BERTH_NUM; ++berthID) {
                if (map->getBerthDistance(berthID, **it) < DISTANCE_INF) {
                    if (PARAMS.ALLOW_CROSS_AREA_GET_GOODS || berthAndBerthInSameWorkGroup(area, berthID)) {
                        berthGoods[berthID].push_back(*it);
                    }
                }
            }
        }
        freeBoatsNum = 0;
        for (auto const &boatDispatcher : *boatDispatchers) { // 更新船只空闲数量
            freeBoatsNum += boatDispatcher.isFree();
        }
    }
    /**
     * @brief 给指定机器人分配任务
     * @param robot 指定机器人
     * @return PlanForLand 分配的任务
     */
    PlanForLand allocatePlan(const Robot *robot) override {
        auto arrivalBerths = map->getArrivalBerths(*robot);
        PlanForLand bestPlan;
        float bestValue = 0.0;
        for (auto berthID : arrivalBerths) {
            if ((*berths)[berthID].isLocked() || (!PARAMS.ALLOW_CROSS_AREA_GET_GOODS && !robotAndBerthInSameWorkGroup(robot->id, berthID))) {
                continue;
            }
            for (auto it = berthGoods[berthID].begin(); it != berthGoods[berthID].end();) {
                if ((*it) == nullptr || !(*it)->isAvailable(frameID + map->getBerthDistance(berthID, **it) + PARAMS.ROBOT_MOVE_MARGIN_TIME)) {
                    it = berthGoods[berthID].erase(it);
                } else {
                    float goodsValue = getGoodsValue(frameID, robot, **it, (*berths)[berthID]);
                    if (goodsValue > bestValue) {
                        bestValue = goodsValue;
                        bestPlan.berthID = berthID;
                        bestPlan.goods = *it;
                    }
                    ++it;
                }
            }
        }
        if (std::abs(bestValue) > std::numeric_limits<float>::epsilon()) {
            bestPlan.setValidate(true);
        }
        return bestPlan;
    }
    /**
     * @brief 给指定船分配任务
     * @param boat 指定船
     * @return PlanForSea 分配的任务
     */
    PlanForSea allocatePlan(const Boat *boat) override {
        static const std::vector<int> berthIDs{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, VIRTUAL_BERTH_ID};
        std::priority_queue<PlanForSea> plans;
        for (auto id: berthIDs) {
            float value = getBerthValue(frameID, id, boat);
            if (std::abs(value) < std::numeric_limits<float>::epsilon()) { // 为0则不入队
                continue;
            }
            PlanForSea plan = id == VIRTUAL_BERTH_ID ? PlanForSea(id, frameID + getBerthDistance(boat->berthID), false) : 
                PlanForSea(id, frameID + getBerthDistance(boat->berthID, id) + (*berths)[id].loadTime, false);
            plan.setValue(value);
            plans.push(plan);
        }
        if (plans.empty()) {
            return PlanForSea();
        }
        PlanForSea ret = plans.top();
        int remainTime = TIME_FRAME_MAX - ret.leaveTime - 1; // 船卖货后的剩余时间
        if (!boat->isFull() && remainTime < minPlanTime) { // 如果后面不会来船且船返航后来不及执行新的任务，船待到最后再走
            if (ret.berthID == VIRTUAL_BERTH_ID && getCommingBoatNum(boat->berthID) == 0) { // 如果方案是回虚拟点，如果后面没有来船，改方案为原地等待，并设置自动返航
                ret.berthID = boat->berthID;
                ret.autoSell = true;
            }
            ret.leaveTime = TIME_FRAME_MAX - 1 - (*berths)[ret.berthID].distance - 1; // 最后一刻再回
        }
        addCommingBoat(ret.berthID, ret.leaveTime - (*berths)[ret.berthID].loadTime);
        return ret;
    }
};

#endif // __PLANNER_WORK_GROUP_HPP__